/*
 * Test of sensors: display inputs from various sensors
 */

# include "init_systems.h"
# include "log.h"
# include <rrcon.h>
# include <time_tools.h>

#include "sensors_read.h"
#include "sensors_print.h"

using namespace sensors_test;
using namespace kthrobot;
using namespace std;

int main (int argc, char **argv) {
    // Init
    Init::RegisterCommon();
    RRCon::Register();
    if (!Init::Systems(argc, argv)) return 1;

    {
    // Setup the tasks
    SensorsReader sensors_reader;
    SensorsPrinter sensors_printer(sensors_reader.get_queue(),sensors_reader.get_angles_queue());

    // Start the tasks
    sensors_reader.Start();
    sensors_printer.Start();
    }

    // Shutdown
    Init::Shutdown();
    return 0;
}
